#include "MCT8316ADriver.h"
#include "bsp_dwt.h"
static MCT8316A_Instance_s *MCT8316A_Instance = NULL;

uint8_t MCT8316A_CRC(uint8_t *data, uint8_t len)
{

}

/* 注意LSB ，低位先行 */
void MCT8316A_SetReg(MCT8316A_Instance_s *instance, MCT8316A_reg_s *reg)
{
    uint8_t tx_buff[9];
    instance->iic_instance->dev_address = 0x00;// Target address and write bit

    tx_buff[0] = OP_W | CRC_DIS | DLEN_32; // Control word 0
    tx_buff[1] = 0x00; // Control word 1
    tx_buff[2] = reg->addr; // Register address
    tx_buff[3] = reg->data & 0xFF; // Data LSB
    tx_buff[4] = (reg->data >> 8) & 0xFF; // Data
    tx_buff[5] = (reg->data >> 16) & 0xFF; // Data
    tx_buff[6] = (reg->data >> 24) & 0xFF; // Data MSB
    IICTransmit(instance->iic_instance, tx_buff, 7, IIC_SEQ_RELEASE);
}
void MCT8316A_GetReg(MCT8316A_Instance_s *instance, MCT8316A_reg_s *reg)
{
    uint8_t tx_buff[9];
    memset(tx_buff, 0, 9);
    instance->iic_instance->dev_address = 0x00;// Target address and write bit
    tx_buff[0] = DLEN_32; // Control word 0
    tx_buff[1] = 0x00; // Control word 1
    tx_buff[2] = ALGO_CTRL1_ADDR;
    tx_buff[3] = 0x80; // Data LSB
    IICTransmit(instance->iic_instance, tx_buff, 7, IIC_SEQ_RELEASE);
    tx_buff[2] = reg->addr; // Register address
    tx_buff[0] = OP_R | CRC_DIS | DLEN_32; // Control word 0
    DWT_Delay(0.1);
    IICTransmit(instance->iic_instance, tx_buff, 3, IIC_SEQ_RELEASE);
    instance->iic_instance->dev_address = 0x01;// Target address and read bit
    uint8_t rx_buff[4];
    memset(rx_buff, 0, sizeof(rx_buff));
    DWT_Delay(0.1);
    IICReceive(instance->iic_instance, rx_buff, 4, IIC_SEQ_RELEASE);
    reg->data = rx_buff[0] | (rx_buff[1] << 8) | (rx_buff[2] << 16) | (rx_buff[3] << 24);
}

MCT8316A_Instance_s *MCT8316A_Init(MCT8316A_conf_s *conf)
{
    MCT8316A_Instance_s *instance = (MCT8316A_Instance_s *)malloc(sizeof(MCT8316A_Instance_s));
    memset(instance, 0, sizeof(MCT8316A_Instance_s));
    instance->iic_instance = IICRegister(&conf->iic_conf);
    instance->measure.speed = 0;
    instance->reg_message.addr = ISD_CONFIG_ADDR;
    instance->reg_message.data = conf->ISD_CONFIG.ISD_SWITCH | conf->ISD_CONFIG.BREAK_SWITCH | conf->ISD_CONFIG.HiZ_SWITCH | conf->ISD_CONFIG.RVS_SWITCH | conf->ISD_CONFIG.RESYNC_SWITCH | conf->ISD_CONFIG.STAT_BRK_SWITCH  | conf->ISD_CONFIG.STAT_DETECT_THR;
    instance->reg_message.data = 0x6F04C180;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = MOTOR_STARTUP1_ADDR;
    instance->reg_message.data = conf->MOTOR_STARTUP1.MTR_STARTUP | conf->MOTOR_STARTUP1.ALIGN_RAMP_RATE | conf->MOTOR_STARTUP1.ALIGN_CURR_THR | conf->MOTOR_STARTUP1.IPD_CLK_FREQ | conf->MOTOR_STARTUP1.IPD_CURR_THR | conf->MOTOR_STARTUP1.ALIGN_TIME | 
    conf->MOTOR_STARTUP1.IPD_RLS_MODE | conf->MOTOR_STARTUP1.IPD_ADV_ANGLE | conf->MOTOR_STARTUP1.IPD_REPEAT | conf->MOTOR_STARTUP1.SLOW_FIRST_CYC_FREQ;
    // instance->reg_message.data = 0x35489195;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = MOTOR_STARTUP2_ADDR;
    // instance->reg_message.data = 0x1890B990 & 0x00001FC0;
    instance->reg_message.data = conf->MOTOR_STARTUP2.OL_current_limit_conf | conf->MOTOR_STARTUP2.OL_ILIMIT | conf->MOTOR_STARTUP2.OL_DUTY_LIMIT | conf->MOTOR_STARTUP2.OL_ACC_A1 | conf->MOTOR_STARTUP2.OL_ACC_A2 | conf->MOTOR_STARTUP2.MIN_DUTY | 
    conf->MOTOR_STARTUP2.OPN_CL_HANDOFF_THR | conf->MOTOR_STARTUP2.AUTO_HANDOFF | conf->MOTOR_STARTUP2.FIRST_CYCLE_FREQ_SEL;
    // instance->reg_message.data = 0x1A295990;
    MCT8316A_SetReg(instance, &instance->reg_message);
    
    instance->reg_message.addr = CLOSED_LOOP1_ADDR;
    instance->reg_message.data = conf->CLOSED_LOOP1.COMM_CONTROL | conf->CLOSED_LOOP1.CL_ACC | conf->CLOSED_LOOP1.CL_DEC | conf->CLOSED_LOOP1.PWM_FREQ_OUT | conf->CLOSED_LOOP1.LD_ANGLE_POLARITY | conf->CLOSED_LOOP1.LD_ANGLE;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = CLOSED_LOOP2_ADDR;
    instance->reg_message.data = conf->CLOSED_LOOP2.FG_SEL | conf->CLOSED_LOOP2.FG_DIV_FACTOR | conf->CLOSED_LOOP2.CBC_ILIMIT | conf->CLOSED_LOOP2.FG_CONFIG | conf->CLOSED_LOOP2.FG_BEMF_THR | conf->CLOSED_LOOP2.MTR_STOP | 
    conf->CLOSED_LOOP2.MTR_STOP_BRK_TIME | conf->CLOSED_LOOP2.AVS_SWITCH | conf->CLOSED_LOOP2.ACT_SPIN_BRK_THR | conf->CLOSED_LOOP2.BRAKE_DUTY_THRESHOLD;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = CLOSED_LOOP3_ADDR;
    instance->reg_message.data = conf->CLOSED_LOOP3.BEMF_THRESHOLD2 | conf->CLOSED_LOOP3.BEMF_THRESHOLD1 | conf->CLOSED_LOOP3.DYN_DEGAUSS | conf->CLOSED_LOOP3.DEGAUSS_MAX_WIN | conf->CLOSED_LOOP3.INTEG_ZC_METHOD | 
    conf->CLOSED_LOOP3.DEGAUSS_MAX_WIN | conf->CLOSED_LOOP3.DYN_DGS_FILT_COUNT | conf->CLOSED_LOOP3.DYN_DGS_LOWER_LIM | conf->CLOSED_LOOP3.DYN_DGS_UPPER_LIM_VM | conf->CLOSED_LOOP3.INTEG_CYCL_THR_HIGH | 
    conf->CLOSED_LOOP3.INTEG_DUTY_THR_HIGH | conf->CLOSED_LOOP3.INTEG_CYCL_THR_LOW | conf->CLOSED_LOOP3.INTEG_DUTY_THR_LOW;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = 0x0000008C;
    instance->reg_message.data = 0x000CE944;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = CONST_SPEED_ADDR;
    instance->reg_message.data = conf->CONST_SPEED.CLOSED_LOOP_MODE | conf->CONST_SPEED.SPD_POWER_KP | conf->CONST_SPEED.SPD_POWER_KI | conf->CONST_SPEED.SPD_POWER_V_MAX | conf->CONST_SPEED.SPD_POWER_V_MIN;
    // instance->reg_message.data = 0x00A00504;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = CONST_PWR_ADDR;
    instance->reg_message.data = conf->CONST_PWR.MAX_POWER | conf->CONST_PWR.MAX_SPEED;
    // instance->reg_message.data = 0x5DC04C84;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = 0x00000096;
    instance->reg_message.data = 0x36DB6DA6;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = 0x00000098;
    instance->reg_message.data = 0x36DB6D80;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = TRAP_CONFIG1_ADDR;
    instance->reg_message.data = 0x054BA106 & 0xFF3FFFFF | conf->TRAP_CONFIG1.OL_HANDOFF_CYCLES;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = 0x0000009C;
    instance->reg_message.data = 0x62880000;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = FAULT_CONFIG1_ADDR;
    instance->reg_message.data = 0x78F43025 & 0xffffff87 | conf->FAULT_CONFIG1.MTR_LCK_MODE;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = FAULT_CONFIG2_ADDR;
    instance->reg_message.data = conf->FAULT_CONFIG2.LOCK1 | conf->FAULT_CONFIG2.LOCK2 | conf->FAULT_CONFIG2.LOCK3 | conf->FAULT_CONFIG2.LOCK_ABN_SPEED | conf->FAULT_CONFIG2.LOSS_SYNC_TIMES | conf->FAULT_CONFIG2.NO_MTR_THR;
    instance->reg_message.data = 0x0003A009;
    instance->reg_message.data = instance->reg_message.data | conf->FAULT_CONFIG2.LOCK1 | conf->FAULT_CONFIG2.LOCK2 | conf->FAULT_CONFIG2.LOCK3 | conf->FAULT_CONFIG2.LOCK_ABN_SPEED | conf->FAULT_CONFIG2.LOSS_SYNC_TIMES | conf->FAULT_CONFIG2.NO_MTR_THR;
    MCT8316A_SetReg(instance, &instance->reg_message);

    instance->reg_message.addr = GD_CONFIG1_ADDR;
    instance->reg_message.data = 0x1C440000;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = GD_CONFIG2_ADDR;
    instance->reg_message.data = 0x14000000;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = PIN_CONFIG1_ADDR;
    instance->reg_message.data = conf->PIN_CONFIG1.SPD_CTRL_MODE | conf->PIN_CONFIG1.DACOUT1_VAR_ADDR | conf->PIN_CONFIG1.DACOUT2_VAR_ADDR;
    instance->reg_message.data = 0x2D720604;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->reg_message.addr = PIN_CONFIG2_ADDR;
    instance->reg_message.data = 0x080C0000;
    MCT8316A_SetReg(instance, &instance->reg_message);
    instance->CRC_buff = 0;
    instance->reg_message.addr = ALGO_CTRL1_ADDR;
    instance->reg_message.data = 0x80000000;
    MCT8316A_SetReg(instance, &instance->reg_message);
    MCT8316A_Instance = instance;
    return instance;
}